#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
    Examples for the NURBS-Python Package
    Released under MIT License
    Developed by Onur Rauf Bingol (c) 2016-2018
"""

import os
from geomdl import BSpline
from geomdl import utilities
from geomdl import exchange
from geomdl import operations
from geomdl import evaluators
from geomdl.visualization import VisMPL
import matplotlib
import matplotlib.pyplot as plt
import csv
is_first = True
path = os.path.dirname(os.path.realpath(__file__))+'/../data/'
f = csv.reader(open(path+"u_turn_record_daxia.csv", 'r'))
point_list = []
count = 0
resample_delta_s = 0.2
delat_s = 2
temp_delat_s = 0.0
sum_s = 0
for i in f:
    count = count+1
    if count < 2:
        continue
    elif count == 2:
        point_list.append([float(i[0]), float(i[1])])
        last_x = float(i[0])
        last_y = float(i[1])
    else:
        temp_delat_s += ((last_x - float(i[0]))**2 +
                         (last_y - float(i[1]))**2)**0.5
        sum_s += ((last_x - float(i[0]))**2 +
                  (last_y - float(i[1]))**2)**0.5

        if temp_delat_s >= delat_s:
            temp_delat_s = 0
            point_list.append([float(i[0]), float(i[1])])
        last_x = float(i[0])
        last_y = float(i[1])
# Fix file path
# Create a BSpline (NUBS) curve instance
curve = BSpline.Curve()

# Set up the curve
curve.degree = 4

curve.ctrlpts = point_list


# Auto-generate knot vector
curve.knotvector = utilities.generate_knot_vector(
    curve.degree, len(curve.ctrlpts))

# Set evaluation delta
curve.delta = resample_delta_s/sum_s/10
# Evaluate curve
curve.evaluate()

f = open(path+'sm.csv', 'w')
dlc_writer = csv.writer(f)
dlc_writer.writerow(["x", "y", "theta", "kappa", "v", "s", "a"])
sum_u = 0.0
curvature_list = []
x_list = []
y_list = []
while sum_u < 1.0:
    ders2 = curve.derivatives(sum_u, 2)
    sum_u = sum_u+resample_delta_s/sum_s
    curvature = (ders2[1][0]*ders2[2][1]-ders2[1][1] *
                 ders2[2][0])/((ders2[2][0]**2+ders2[2][1]**2)**1.5)
    curvature_list.append(curvature)
    x_list.append(ders2[0][0])
    y_list.append(ders2[0][1])
    # Plot the control point polygon and the evaluated curve
plt.plot(curvature_list)
plt.show()
plt.plot(x_list, y_list)
plt.show()
vis_comp = VisMPL.VisCurve2D()
curve.vis = vis_comp
curve.render()

# Evaluate derivatives at u = 0.6
ders1 = curve.derivatives(0.6, 2)
print(ders1)
